import lejos.nxt.*;

public class Balancer {
	protected LightSensor LightSensor;
	protected Motor LeftMotor;
	protected Motor RightMotor;
	protected PID PID;
	protected double LightBalancedValue;
	
	public Balancer(Motor leftMotor, Motor rightMotor, SensorPort lightSensorPort, PID PID) {
		this.LightBalancedValue = 0;
		this.LeftMotor = leftMotor;
		this.RightMotor = rightMotor;
	
		// Turn off the smooth accelerators - we need fast response time
		this.LeftMotor.smoothAcceleration(false); 
		this.RightMotor.smoothAcceleration(false);

		// Set the PID
		this.PID = PID;
	
		// Initialize the sensors
		this.LightSensor = new LightSensor(lightSensorPort);
	}
	
	public void Update() {
		double PIDResult = this.PID.Get(this.LightBalancedValue, this.LightSensor.getLightValue());
		System.out.println("PID: " + PIDResult);
		this.LeftMotor.setSpeed((int)PIDResult);
		this.RightMotor.setSpeed((int)PIDResult);
		if (PIDResult > 0) {
			this.LeftMotor.forward();
			this.RightMotor.forward();
		}
		else {
			this.LeftMotor.backward();
			this.RightMotor.backward();
		}
	}
	
	public void Stop() {
		this.LeftMotor.stop();
		this.RightMotor.stop();
	}
	
	public void SetLightValue(double lightValue) {
		this.LightBalancedValue = lightValue;
	}
	
	public void SetPID(PID PID) {
		this.PID = PID;
	}
	
	public void Calibrate() {
    	System.out.println("Calibrating...");
    	int LightBalancedCount = 0;
    	while (LightBalancedCount < 1000) {
    		LightBalancedCount++;
    		this.LightBalancedValue = (this.LightBalancedValue * (LightBalancedCount - 1) + this.LightSensor.getLightValue()) / LightBalancedCount;
    	}
    	System.out.println("  Done.");
    	System.out.println("Doing extra,");
    	System.out.println("  press to stop.");
    	while (!Button.BUTTONS[0].isPressed() && LightBalancedCount < 10000) {
    		LightBalancedCount++;
    		this.LightBalancedValue = (this.LightBalancedValue * (LightBalancedCount - 1) + this.LightSensor.getLightValue()) / LightBalancedCount;
    	}
    	System.out.println("Calibrated:");
    	System.out.println("Front: " + this.LightBalancedValue);
    	System.out.println("Press button...");
    	Button.waitForPress();
	}
}